import os
import subprocess

# Source ROS环境
ros_setup_path = "/opt/ros/noetic/setup.bash"
mavros_ws_setup_path = "/home/wushuanglang/mavros_ws/devel/setup.bash"

# 动态 source setup.bash
command = ['bash', '-c', f"source {ros_setup_path} && source {mavros_ws_setup_path} && env"]
proc = subprocess.Popen(command, stdout=subprocess.PIPE)
for line in proc.stdout:
    (key, _, value) = line.decode("utf-8").partition("=")
    os.environ[key] = value.strip()

import rospy
from std_msgs.msg import String

# 后续ROS相关代码
def callback(data):
    rospy.loginfo("I heard %s", data.data)

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("chatter", String, callback)
    rate = rospy.Rate(2)  # 设定频率为10Hz
    while not rospy.is_shutdown():
        # 这里可以放置其他代码或处理逻辑
        rospy.loginfo("节点仍在运行...")
        rate.sleep()  # 让循环按照设定的频率休眠（和回调函数的刷新速率无关）
    # python中不需要手动刷新回调函数，回调函数的执行速率取决于发布速率(只要节点不退出)
    # rospy.spin() # 保持节点活跃，有消息时调用回调函数

if __name__ == '__main__':
    listener()

